/*
 * usb_card.c
 *
 * Created: 27.09.2012 13:05:57
 *  Author: bardbakk
 */ 

#define F_CPU 8000000

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/delay.h>
#include <stdlib.h>
#include <string.h>

#include "ADC.h"
#include "OLED.h"
#include "JOY.h"
#include "DISPLAY.h"
#include "MENU.h"
#include "CAN.h"
#include "JOY.h"
#include "CHANGE_PD.h"
#include "PLAY.h"

int main(void)
{
	//OLED startup delay
	_delay_ms(1000);
	
	CAN_message_t *msg = NULL;
	CAN_message_t *recv_msg = calloc(1, sizeof(CAN_message_t));
	CAN_message_t send_msg;
	uint8_t loopCnt = 0;
	uint8_t done = FALSE;
	uint8_t goal = FALSE;
	uint8_t first = TRUE;
	USB_state = BROWSE;
	JOY_Pos pos;
	sei();
	
	JOY_button_init();
	MENU_init();
	CAN_init();
	
	//Init timer1 with 1/64 prescaler
	BIT_SET(TCCR3B,CS30);
	BIT_SET(TCCR3B,CS31);
	
	//Request PI values
	send_msg.length = 0;
	send_msg.id = CAN_REQUEST_PI_MSG;
	CAN_message_send(send_msg);
	
    while(1)
    {	
		if(CAN_msg_rcvd)
		{
			msg = CAN_message_receive();
			memcpy(recv_msg, msg, sizeof(CAN_message_t));
			free(msg);
		}		
		
		//Print display with 60 Hz
		if(TCNT3 > 2084)
		{
			
			DISPLAY_print();
			TCNT3 = 0;
			loopCnt++;
		}
		
		
		//Read CAN
		switch(recv_msg->id)
		{
			case CAN_READ_PI_EEPROM_MSG:
				CHANGE_PD_P_gain = recv_msg->data[0];
				CHANGE_PD_D_gain = recv_msg->data[1];
				break;
			case CAN_GOAL_MSG:
				goal = TRUE;
				break;
			case 200:
				DISPLAY_int(recv_msg->data[0], 1, 0);
				DISPLAY_int(recv_msg->data[1], 1, 5);
				DISPLAY_int(recv_msg->data[2], 1, 10);
				DISPLAY_int(recv_msg->data[3], 1, 15);
		}
		
		//State of USB card
		switch(USB_state)
		{
			case BROWSE:
				MENU_browse();	
				break;
			case CHANGE_PD:
				done = CHANGE_PD_change();
				if(done)
				{
					send_msg.data[0] = CHANGE_PD_P_gain;
					send_msg.data[1] = CHANGE_PD_D_gain;
					send_msg.length = 2;
					send_msg.id = CAN_CHANGE_PI_MSG;
					CAN_message_send(send_msg);
					
					USB_state = BROWSE;
					MENU_init();
					
					done = FALSE;
				}
				break;
			case SRAM_TEST:
				send_msg.id = CAN_SRAM_TEST_MSG;
				send_msg.length = 0;
				CAN_message_send(send_msg);
				
				USB_state = BROWSE;
				break;
			case PLAY:
				//Send joystick position with 5 Hz over CAN
				if(loopCnt >= 2)
				{
					pos = JOY_read();
					send_msg.data[0] = pos.x;
					send_msg.data[1] = pos.y;
					send_msg.data[2] = pos.dir;
					send_msg.data[3] = JOY_button_pressed();
					send_msg.length = 4;
					send_msg.id = CAN_CONTROL_MSG;
					CAN_message_send(send_msg);
					loopCnt = 0;
				}
				
				//Run PLAY state machine with goal info
				done = PLAY_play(goal, JOY_button_pressed());	
				goal = FALSE;
				
				if(done)
				{
					DISPLAY_clear();
					USB_state = BROWSE;
					MENU_init();
					done = FALSE;
				}
				
				break;
			case TRACK:
				if(first)
				{
					send_msg.length = 0;
					send_msg.id = CAN_TRACK_MSG;
					CAN_message_send(send_msg);
					first = FALSE;
				}
				//Run PLAY state machine with goal info
				done = PLAY_play(goal, JOY_button_pressed());	
				goal = FALSE;
				
				if(done)
				{
					DISPLAY_clear();
					USB_state = BROWSE;
					MENU_init();
					done = FALSE;
					first = TRUE;
				}
				break;
		}		
	recv_msg->id = 255;
	}
}